
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srv_channels.c
  * @author     baiyang
  * @date       2021-12-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "srv_channel.h"

#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#ifndef NUM_RC_CHANNELS
#define NUM_RC_CHANNELS 16
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
srv_channels srvs;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void srv_channels_ctor()
{
    // 参数赋值
    srvs.default_rate = 50;

    srvs.channels = srvs.obj_channels;

    // setup ch_num on channels
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        srv_channel_ctor(&srvs.channels[i], i);
    }
}

// SRV_Channels initialization
void srv_channels_init(void)
{
    // initialize BLHeli late so that all of the masks it might setup don't get trodden on by motor initialization
    
}

/*
  save adjusted trims
 */
void srv_channels_save_trim(void)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if (srvs.trimmed_mask & (1U<<i)) {
            // GP TODO: ?
            // 保存参数，因现在没有参数，后续增加
            srvs.channels[i].servo_trim = srvs.channels[i].servo_trim;
        }
    }
    srvs.trimmed_mask = 0;
}

void srv_channels_setup_failsafe_trim_all_non_motors(void)
{
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if (!srv_channel_is_motor(srvs.channels[i].function)) {
            srv_hal_set_failsafe_pwm(1U<<srvs.channels[i].ch_num, srvs.channels[i].servo_trim);
        }
    }
}

/*
  run calc_pwm for all channels
 */
void srv_channels_calc_pwm(void)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        // check if channel has been locked out for this loop
        // if it has, decrement the loop count for that channel
        if (srvs.override_counter[i] == 0) {
            srv_channel_set_override(&srvs.channels[i], false);
        } else {
            srv_channel_set_override(&srvs.channels[i], true);
            srvs.override_counter[i]--;
        }
        if (srv_channel_valid_function(&srvs.channels[i])) {
            srv_channel_calc_pwm(&srvs.channels[i], srvs.functions[srvs.channels[i].function].output_scaled);
        }
    }
}

// set output value for a specific function channel as a pwm value
void srv_channels_set_output_pwm_chan(uint8_t chan, uint16_t value)
{
    if (chan < NUM_SERVO_CHANNELS) {
        srv_channel_set_output_pwm(&srvs.channels[chan], value, false);
    }
}

// set output value for a specific function channel as a pwm value with loop based timeout
// timeout_ms of zero will clear override of the channel
// minimum override is 1 MAIN_LOOP
void srv_channels_set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms)
{
    if (chan < NUM_SERVO_CHANNELS) {
        //const uint32_t loop_period_us = AP::scheduler().get_loop_period_us();
        const uint32_t loop_period_us = 5000;
        // round up so any non-zero requested value will result in at least one loop
        const uint32_t loop_count = ((timeout_ms * 1000U) + (loop_period_us - 1U)) / loop_period_us;
        srvs.override_counter[chan] = math_constrain_int32(loop_count, 0, UINT16_MAX);
        srv_channel_set_override(&srvs.channels[chan], true);
        const bool had_pwm = srv_channel_get_have_pwm_mask() & (1U<<chan);
        srv_channel_set_output_pwm(&srvs.channels[chan], value,true);
        if (!had_pwm) {
            // clear the have PWM mask so the channel will default back to the scaled value when timeout expires
            // this is also cleared by set_output_scaled but that requires it to be re-called as some point
            // after the timeout is applied
            // note that we can't default back to a pre-override PWM value as it is not stored
            // checking had_pwm means the PWM will not change after the timeout, this was the existing behaviour
            srv_channel_have_pwm_mask(chan);
        }
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void srv_channels_zero_rc_outputs()
{
    /* Send an invalid signal to the motors to prevent spinning due to
     * neutral (1500) pwm pulse being cut short.  For that matter,
     * send an invalid signal to all channels to prevent
     * undesired/unexpected behavior
     */
    srv_channels_cork();
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
        srv_hal_write(i, 0);
    }
    srv_channels_push();
}

/*
  return true if a channel should be available as a GPIO
 */
bool srv_channels_is_GPIO(uint8_t channel)
{
    if (srv_channels_channel_function(channel) == k_GPIO) {
        return true;
    }
    if ((srvs.gpio_mask & (1U<<channel)) != 0) {
        // user has set this channel in SERVO_GPIO_MASK
        return true;
    }
    return false;
}

// Set E - stop
void srv_channels_set_emergency_stop(bool state)
{
    srvs.emergency_stop = state;
}

// get E - stop
bool srv_channels_get_emergency_stop()
{
    return srvs.emergency_stop;
}

/*------------------------------------test------------------------------------*/


